#include "mm.h"
#include "sched.h"

void init_page_table()
{
	int i;
	uint32_t vpn2=0x0;
	uint32_t pfn=3;
	for(i=0;i<64;i++,vpn2++,pfn++)
	{
	 	pgtable[vpn2<<1].pte = pfn<<6|PTE_C<<3|PTE_D<<2|PTE_V<<1|PTE_G;
	 	pgtable[vpn2<<1|1].pte = pfn<<6|PTE_C<<3|PTE_D<<2|PTE_V<<1|PTE_G;		
	}
}
void init_TLB(void)
{
    int i;
	uint32_t vpn2 = 0;
	uint32_t asid = 0,pagemask = 0,index = 0;
	for(i = 0; i < 64 ;i ++,vpn2 ++,index ++)
    {
		//printk("> [INIT] Virtual memory initialization succeeded[%d].\n",i);
		set_cp0_entryhi(vpn2<<13|asid&0xff);
		set_cp0_entrylo0(pgtable[vpn2 << 1].pte);
		set_cp0_entrylo1(pgtable[vpn2 << 1 | 1].pte);		
		set_cp0_pagemask(pagemask);
		set_cp0_index(index);
		tlbwi_operation();
	}
}
void do_TLB_helper()
{
	uint64_t entryhi  = get_cp0_entryhi();
    uint64_t context  = get_cp0_context(); 
    set_cp0_entryhi(context << 9 | entryhi & 0xff);
	tlbp_operation();
	uint32_t index = get_cp0_index();
	if((context>>3|1)>=512)
		kprintf("address error\n");
	else if(index & 0x80000000)
	{
		//do_TLB_Refill();
		set_cp0_entryhi(((context << 9) & 0xffffff00) | ( current_running->pid & 0xff));
		set_cp0_entrylo0(current_running->pt[context >>3].pte | (PTE_C<<3));
		set_cp0_entrylo1(current_running->pt[context >>3 | 1].pte | (PTE_C<<3));	
		tlbwr_operation();
	}
	else
	{
		if(!(current_running->pt[context >> 3].pte & (PTE_V<<1)))
		{
			int i ;
			for(i = 0; pframe_table[i].valid != 1; i++);
			pframe_table[i].valid = 0;
			int pfn = pframe_table[i].pfn;
			current_running->pt[context >> 3].pte = pfn<<6|PTE_C<<3|PTE_D<<2|PTE_V<<1|PTE_G;
	 		current_running->pt[context >> 3|1].pte = pfn<<6|PTE_C<<3|PTE_D<<2|PTE_V<<1|PTE_G;
			current_running->pt[context >> 3].valid = 1;
	 		current_running->pt[context >> 3|1].valid = 1;
		}
		set_cp0_entryhi(((context << 9) & 0xffffff00) | ( current_running->pid & 0xff));
		set_cp0_entrylo0(current_running->pt[context >> 3].pte);
        set_cp0_entrylo1(current_running->pt[context >> 3 | 1].pte);
        tlbwi_operation();
	}
}

void physical_frame_initial(void)
{
	int i;
	int pfn = 3;
	for (i = 0 ; i < PFRAME_NUM ; i ++)
	{
		pframe_table[i].pfn = pfn;
		pframe_table[i].valid = 1;
		pfn += 2;
	}
}
void recycle_pframe(uint32_t pfn)
{
	int i;
	for (i = 0 ; pframe_table[i].pfn != pfn ; i ++);
	pframe_table[i].valid = 1;
}
